
#include "modules/planning/scenarios/traffic_light_left_turn_waiting_zone/stage_intersection_cruise.h"
#include "modules/planning/scenarios/traffic_light_left_turn_waiting_zone/traffic_light_left_turn_waiting_zone.h"
#include "cyber/common/log.h"

namespace apollo {
namespace planning {

StageResult TrafficLightLeftTurnWaitingZoneStageIntersectionCruise::Process(
        const common::TrajectoryPoint& planning_init_point,
        Frame* frame) {
    const auto* context = GetContextAs<TrafficLightLeftTurnWaitingZoneContext>();
    const auto& scenario_config = context->scenario_config;
    auto& reference_line_info = frame->mutable_reference_line_info()->front();
    reference_line_info.LimitCruiseSpeed(scenario_config.cruise_speed());
    StageResult result = ExecuteTaskOnReferenceLine(planning_init_point, frame);
    auto done_traffic_light = injector_->planning_context()->mutable_planning_status()->mutable_traffic_light();
    done_traffic_light->mutable_done_traffic_light_overlap_id()->Clear();
    done_traffic_light->add_done_traffic_light_overlap_id(context->forward_traffic_light_id);
    done_traffic_light->add_done_traffic_light_overlap_id(context->left_turn_traffic_light_id);
    if (result.HasError()) {
        AERROR << "TrafficLightLeftTurnWaitingZoneStageIntersectionCruise "
               << "plan error";
    }

    bool stage_done = CheckDone(*frame, injector_->planning_context(), true);
    if (stage_done) {
        return FinishStage();
    }
    return result.SetStageStatus(StageStatusType::RUNNING);
}

StageResult TrafficLightLeftTurnWaitingZoneStageIntersectionCruise::FinishStage() {
    return FinishScenario();
}

}  // namespace planning
}  // namespace apollo
